package control.ioio.quadricopter;

import java.io.IOException;
import java.io.InputStream;
import ioio.lib.api.IOIO;
import ioio.lib.api.IOIOFactory;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.Uart;
import ioio.lib.api.exception.ConnectionLostException;
import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.view.View;
import android.view.WindowManager;
import android.widget.EditText;
import android.widget.TextView;
import android.widget.ToggleButton;

public class MainActivity extends Activity implements SensorEventListener {

	/*
	 *  Process related variables
	 *  
	 */

	/** The sensor manager to use sensors **/
	private SensorManager sm = null;

	/** The thread that interacts with the IOIO */
	private IOIOThread ioio_thread_;

	/** This manage the output file data **/
	private DataRegister dataRegister;  

	/** The I/O pines and peripherals used in the IOIO **/
	final int portA = 11;
	final int portB = 12;
	final int portC = 13;
	final int portD = 14;
	final int portRx = 4;
	final int portTx = 5;

	/** The PWM peripherals objects **/
	private PwmOutput pwmA;
	private PwmOutput pwmB;
	private PwmOutput pwmC;
	private PwmOutput pwmD;

	/** The PWM frequency in Hz **/
	final int pwmFrec = 220;

	/** The UART port object **/
	private Uart uart;

	/** The stream where the UART input is obtained **/
	private InputStream in;

	/** Variables used to compute the phone orientation and acceleration **/
	private float[] dataGyro 		= new float[3];
	private float[] dataAcc 		= new float[3];
	private float[] dataMag 		= new float[3];
	private float[] mR	 			= new float[9];
	private float[] orientation 	= new float[3];
	private float[] ref_orientation = { 0, 0, 0 };
	private float[] auxOrientation 	= { 0, 0, 0 };
	private long t_last				= 0;

	private PID angle1PID; 

	/** Auxiliary variables for data conversion **/
	final float N2S 		= 0.000000001f;

	/** Fixed sample time (CURRENTY NOT IN USE) **/
	final long fixedTimeNanos 	= 500000;

	/** Determine the cut-off frequency of the complementary filter */
	final float percent = 0.95f;

	/*
	 *  GUI variables
	 *  
	 */

	/** This text display the IOIO state (connected, waiting, etc) */
	private TextView IOIOstate;

	/** This text display the filtered angles **/
	private TextView angle1;
	private TextView angle2;
	private TextView angle3;

	/** This text display the real sample time **/
	private TextView sampleTime;

	private TextView varM;

	/** This text display the xBee input stream **/
	private TextView xbeeStream;


	/** Use for save file data **/
	private ToggleButton saveData;
	private EditText fileName;


	/*
	 *  Methods
	 */

	/** Called when the activity is first created **/
	@Override
	public void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.main);

		/** Add flags to prevent phone get lock when running **/ 
		getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);

		/**  Acquire widgets from GUI **/
		IOIOstate 	= (TextView) findViewById(R.id.title);
		angle1 		= (TextView) findViewById(R.id.angle1);
		angle2 		= (TextView) findViewById(R.id.angle2);
		angle3 		= (TextView) findViewById(R.id.angle3);
		sampleTime 	= (TextView) findViewById(R.id.sampleTime);

		varM = (TextView) findViewById(R.id.variableManipulada);

		xbeeStream = (TextView) findViewById(R.id.xbeeStream);

		/** Acquire sensor manager from system **/
		sm = (SensorManager) getSystemService(SENSOR_SERVICE);


		angle1PID = new PID(1.0f, 2f, 1000f, 10.0f, 1, 100f, -100f);

		/** Acquire file output related widgets **/
		saveData = (ToggleButton) findViewById(R.id.grabarDatos);
		fileName = (EditText) findViewById(R.id.nombreArchivo);
		
		/** Called when saveData button is pressed **/
		saveData.setOnClickListener(new View.OnClickListener() {
			@Override
			public void onClick(View v) {

				if(saveData.isChecked()){
					dataRegister = new DataRegister(fileName.getText().toString());
				}
				else{
					dataRegister.closeFile();
					dataRegister = null;
				}
			}
		});

	}

	/** 
	 * Required method to implements SensorEventListener, this override don't change 
	 * behavioral of the primitive method
	 *  
	 **/
	@Override
	public void onAccuracyChanged(Sensor arg0, int arg1) {
		// TODO Auto-generated method stub
	}

	/**
	 * Method called when SensorChange event is tick, here we estimate the device
	 * orientation, then the duty cycle of the PWM outputs are update using PID
	 * controllers 
	 * 
	 * @param event This object have all data related to event occurrence
	 */
	@Override
	public void onSensorChanged(SensorEvent event) {

		long t_initial = System.nanoTime();

		/** Calculate the time between the last sample and the current task **/
		float dt = (t_initial - t_last) * N2S;
		
		sampleTime.setText("Sample time: " + (t_initial-t_last) + " [ns]");
		
		/** Acquire the set-points from UART **/
		try {
			ioio_thread_.readUart();
		} catch (IOException e1) {
			e1.printStackTrace();
		}
		
		/** Save the data in the correct variable according to the sensor that rise the
		 * event
		 */
		switch (event.sensor.getType()) {
		case Sensor.TYPE_GYROSCOPE:
			dataGyro = event.values.clone();
			break;
		case Sensor.TYPE_ACCELEROMETER:
			dataAcc = event.values.clone();
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			dataMag = event.values.clone();
			break;
		}

		if (dataAcc != null && dataMag != null && dataGyro != null) {
			/** Calculate the orientation using the acceleration and magnetometer 
			 *  data
			 */
			if (SensorManager.getRotationMatrix(mR, null, dataAcc, dataMag)) {
				SensorManager.getOrientation(mR, auxOrientation);

				if (orientation == null) {
					orientation = auxOrientation.clone();
				} else {
					/**
					 * Apply complementary filter to estimate the orientation, 
					 * this is compute as a weighted sum of: the orientation find with
					 * accelerometer and magnetometer, and, the orientation find with
					 * the integral of gyroscope values (using euler backward method). 
					 * 
					 * The weighted sum acts as a high-pass filter for gyroscope orientation 
					 * measure and as a low-pass filter for acc-mag orientation measure.
					 * 
					 */
					orientation[0] = (percent)
							* (orientation[0] - (dataGyro[2] * dt))
							+ (1.0f - percent) * auxOrientation[0];
					orientation[1] = (percent)
							* (orientation[1] - (dataGyro[0] * dt))
							+ (1.0f - percent) * auxOrientation[1];
					orientation[2] = (percent)
							* (orientation[2] + (dataGyro[1] * dt))
							+ (1.0f - percent) * auxOrientation[2];
				}
			}
		}

		if(dataRegister !=null){
			dataRegister.updateFile(t_initial, orientation, auxOrientation);
		}

		angle1.setText("Angle 1: " + orientation[0] );
		angle2.setText("Angle 2: " + orientation[1] );
		angle3.setText("Angle 3: " + orientation[2] );


		/** Delay the thread to complete the fixed time (CURRENTLY NOT IN USE)**/

		/*
		long delay = fixedTimeNanos - (System.nanoTime() - t_initial);

		if (delay > 0) {
			try {
				Thread.sleep(0, (int)delay);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		 */

		float VM = angle1PID.Update(dt, ref_orientation[2], orientation[2]);
		varM.setText("Manipulated variable: "+ VM);

		/** Test the PWM output **/
		try {
			ioio_thread_.updatePwmDutyCycle(MyMath.map(VM/2, -1.5f, 1.5f, 0f, 1f), MyMath.map(-VM/2, -1.5f, 1.5f, 0f, 1f), 0f, 0f);
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		}

		t_last = System.nanoTime();
	}

	@Override
	protected void onResume() {
		super.onResume();
		System.gc();

		int rate = SensorManager.SENSOR_DELAY_FASTEST;

		/** Get default accelerator sensor, magnetometer and gyroscope from system **/
		Sensor sAcc 	= sm.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		Sensor sMag 	= sm.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
		Sensor sGyro 	= sm.getDefaultSensor(Sensor.TYPE_GYROSCOPE);

		/** Register to Sensor Change event from sensor manager **/
		sm.registerListener(this, sAcc, rate);
		sm.registerListener(this, sMag, rate);
		sm.registerListener(this, sGyro, rate);

		t_last = System.nanoTime();

		/** Star the IOIO thread **/
		ioio_thread_ = new IOIOThread();
		ioio_thread_.start();
	}

	@Override
	protected void onPause() {
		super.onPause();

		/** Unregister the Sensor Change event from sensor manager **/
		sm.unregisterListener(this);

		/** Close ioio_thread**/
		ioio_thread_.abort();
		try {
			ioio_thread_.join();
		} catch (InterruptedException e) {
		}

		if(dataRegister!=null){
			dataRegister.closeFile();
			dataRegister = null;
		}
	}


	/** This class extends thread, here the IOIO board is managed **/
	class IOIOThread extends Thread {

		private IOIO ioio_;
		private boolean abort_ = false;

		/** Thread body. */
		@Override
		public void run() {
			super.run();
			while (true) {
				synchronized (this) {
					if (abort_) {
						break;
					}
					ioio_ = IOIOFactory.create();
				}
				try {
					/** Wait for IOIO board connect **/
					setText(R.string.wait_ioio);
					ioio_.waitForConnect();
					setText(R.string.ioio_connected);

					/** Open PWM outputs **/
					pwmA = ioio_.openPwmOutput(portA, pwmFrec);
					pwmB = ioio_.openPwmOutput(portB, pwmFrec);
					pwmC = ioio_.openPwmOutput(portC, pwmFrec);
					pwmD = ioio_.openPwmOutput(portD, pwmFrec);

					uart = ioio_.openUart(portRx, portTx , 9600, Uart.Parity.NONE,
							Uart.StopBits.ONE);
					in = uart.getInputStream();

				} catch (ConnectionLostException e) {
				} catch (Exception e) {
					ioio_.disconnect();
					break;
				} finally {
					try {
						ioio_.waitForDisconnect();

					} catch (InterruptedException e) {
					}
				}
			}
		}

		synchronized public void abort() {
			abort_ = true;
			if (ioio_ != null) {
				ioio_.disconnect();
			}
		}


		public boolean updatePwmDutyCycle(float dutyA, float dutyB, float dutyC, float dutyD) throws ConnectionLostException {
			if (pwmA != null && pwmB != null && pwmC != null && pwmD != null) {
				pwmA.setDutyCycle(dutyA);
				pwmB.setDutyCycle(dutyB);
				pwmC.setDutyCycle(dutyC);
				pwmD.setDutyCycle(dutyD);
				return true;
			} else
				return false;
		}


		/**
		 * Update all the PWM peripherals duty cycle at the same time 
		 * 
		 * @param widthA
		 *            The width of the A peripheral ON state
		 * @param widthB
		 *            The width of the B peripheral ON state
		 * @param widthC
		 *            The width of the C peripheral ON state
		 * @param widthD
		 *            The width of the D peripheral ON state
		 *                       
		 * @return boolean Return true if successful 
		 */
		public boolean updatePwmWidht(float widthA, float widthB, float widthC, float widthD) throws ConnectionLostException {
			if (pwmA != null && pwmB != null && pwmC != null && pwmD != null) {
				pwmA.setPulseWidth(widthA);
				pwmB.setPulseWidth(widthA);
				pwmC.setPulseWidth(widthA);
				pwmD.setPulseWidth(widthA);
				return true;
			} else
				return false;
		}

		public String readUart() throws IOException{
			if(uart != null){
				int availableBytes = in.available();
				int bufferSize = 1000;

				if (availableBytes > 0) {
					byte[] readBuffer = new byte[bufferSize];
					in.read(readBuffer, 0, availableBytes);
					char[] charstring= (new String(readBuffer, 0, availableBytes)).toCharArray();
					String message = new String(charstring);
					setXbeeTextDisplay(message);
					return message;
				}
				else
					return null;
			}
			else
				return null;
		}

		private void setXbeeTextDisplay(final String mensaje) {
			runOnUiThread(new Runnable() {
				@Override
				public void run() {
					xbeeStream.setText("Xbee stream: "+mensaje);
				}
			});
		}

		/**
		 * Set the text that show the IOIO board state 
		 * 
		 * @param id
		 *            The string ID of the message to present.
		 */
		private void setText(final int id) {
			runOnUiThread(new Runnable() {
				@Override
				public void run() {
					IOIOstate.setText(getString(id));
				}
			});
		}
	}
}